function [Glob_last,hout,Out,Out_msgs,handles] = solveformation (fun,tspan,glob0,out0,handles,varargin)
%����������ⷽ��
%Out:�������ʱ�̵������λ�þ���
%Out_msgs:�������ʱ�̵������ros message��ʽ��
global N N2 host_ip delta_t
global r
global scale
global pos_flag xydesire %pos_flag是记下走到哪条路线O上的标志，它的值为将要到达的点的序号。
global change_signal
global L
global best_list
global zhangai
global des_flag
global xd_estimate yd_estimate
global xdyd
global vr
global ROS_ON

T = length (tspan);
%%%%%%%%%%%%%%%%%匀速直线%%%%%%%%%%%%%%%%%%%
small=0.1;
%small为一个很小的数，用于判断误差范围
xydesire= ones(N, 2);     %针对每组，随便设一个值，用于初始化，但不能是零。
pos_flag= ones(N, 1);    %针对每组，表示到第几个标志点了；
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%障碍物+变队形%%%%%%%%%%%%%%%%%
t=(0:0.02:1)'*2*pi;
%准备障碍物画图
bar= zeros( size(zhangai,1) , 3 );
bar( 1:end, 1:2 ) = zhangai;
bar( :, 3 ) = 2*r(1,1);
% switch N
%     case {1,2,3}
%         bar=[-12  108 2*r(1,1); 108  -12 2*r(1,1); -108 -108 2*r(1,1)];   %这个要改；要不断更新；
%     otherwise
%         bar=[60  60 2*r(1,1); 156  -12 2*r(1,1); -132 -108 2*r(1,1) ];
% end
zoom=0.8;   %obstacles display radius / real radius
for i=1:size(bar,1)
    barx(:,i)=bar(i,1)+bar(i,3)*sin(t)*zoom;
    bary(:,i)=bar(i,2)+bar(i,3)*cos(t)*zoom;    %barx,baryGlob(是障碍物圆的xy坐标；
end
hold on %为什么hold on在前面，是因为前面已经自动创建图窗
%障碍物画图
for i=1:size(bar,1)
    H=fill(barx(:,i),bary(:,i),'m');
%     set(H,'linewidth',1);
end
change_signal = zeros(N, 1);    %标志每组是否太靠近障碍物，是否需要变队形；
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%搜索目标%%%%%%%%%%%%%%%%%%%%%
%目标点画图
%             x,x];       %d-destination
hold on
for i=1:size(xdyd,1)
    plot(xdyd(i,1),xdyd(i,2),'--gp',...
        'LineWidth',2,...
        'MarkerSize',20,...
        'MarkerEdgeColor','b',...
        'MarkerFaceColor',[0.5,0.5,0.5])
end
standard=20;        %靠近destinations的标准值；
des_flag=zeros(N ,size(xdyd,1), T-1);       %des_flag是看是否接近destination的标志；
xd_estimate=zeros(N ,size(xdyd,1) ,N2-2,T-1);        %存下来所有的估计值；N2-2是每组的plane数量；
yd_estimate=zeros(N ,size(xdyd,1) ,N2-2,T-1); 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

No = diff (tspan);   %���  %delta t
T_draw=0.1/delta_t; % draw plot interval
% T_rospub=0.05/delta_t; %ros publish interval
T_rospub=delta_t/delta_t; %ros publish interval %TODO: record less ros message data
n1 = length (glob0);
Glob_last = zeros (n1,N,3); % all states of the system in last time
Glob_last= glob0;

n2 = length (out0);
Out = zeros(n2,N,3,T);
hout = zeros(N2-1,N,3,T);
km1 = zeros (n1,4); % K-T
for i=1:N
    for j=1:3
    Out(:,i,j,1) = out0(:,i,j);
    end
end
color=['r','b','k','m','r','b','k'];
%shape=['+','o','*','s','d','^'];
for i=1:N
    handles.vr_leader(i)=line('xdata',0,'ydata',0,'zdata',0,'Marker','o','Color',color(i));%virtual leader
    handles.car(i)=line('xdata',0,'ydata',0,'zdata',0,'Marker','*','Color',color(i));
    for j=1:4 % inner group drone handles
        handles.drone(i,j)=line('xdata',0,'ydata',0,'zdata',0,'Marker','+','Color',color(i));
        handles.dronelabell(i,j)=text(0,0,[' ',num2str((j+2))]);
    end
    %handles.line(i)=line([Out(2,i,1,1),Out(3,i,1,1)],[Out(2,i,2,1),Out(3,i,2,1)]);
end

axis([-scale,scale,-scale,scale]);
grid on;

topic_name='/gazebo/set_model_states';
msg_type='gazebo_msgs/ModelStates';     %ͨ����ǰ�̵棻

global flag_rosinit
if ~flag_rosinit
    if host_ip == '127.0.0.1'
        rosinit;%local
    else
        rosinit(host_ip);
    end
    flag_rosinit=true;
end
global pub
pub = rospublisher(topic_name,msg_type);
tmp_msgs=rosmessage(msg_type);  
for name_num=1:N %generate name strings for ros message
    tmp_name=strcat('car', int2str(name_num-1));
    tmp_msgs.Name=[tmp_msgs.Name;tmp_name];
    for k=3:N2
        tmp_name=strcat('drone', int2str((name_num-1)*(N2-2)+k-3));
        tmp_msgs.Name=[tmp_msgs.Name;tmp_name];
    end
end

Out_msgs=rosmessage(msg_type);  
for i = 2:T % iteration step
    set(handles.text_console,'String',['simulating...',' t= ',num2str((i-1)*delta_t),' s']);
    t_start = cputime;     
    tt = tspan (i-1);
    num = No(i-1);
    for number=1:N %group number
        for ax=1:3      %x,y,z

            Globi = Glob_last(:,number,ax);
            %%%%%%%%%%%%%%%计算匀速直线速度%%%%%%%%%%%%%%%
            if (abs( xydesire(number, :) )<[small small])*[1 1]' == 2 %这里的2是指有两维都符合误差范围
                pos_flag(number)=pos_flag(number)+1;
            end
            xydesire(number, :)=best_list{number}( pos_flag(number), : ) - squeeze(Glob_last(1,number,1:2))';%是第一组的运动方向
            if ax==1
                Globi( 2 )=vr*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)');   %vr是虚拟领导者速度m/s;Globi( 2 )是虚拟领导者的x速度；
            end
            if ax==2
                Globi( 2 )=vr*xydesire(number, ax)/sqrt(xydesire(number, :)*xydesire(number, :)');   %vr是虚拟领导者速度m/s;Globi( 2 )是虚拟领导者的y速度；
            end
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            
            Outi = Out(:,number,ax,i-1);
            [~,hout(:,number,ax,i),Out(:,number,ax,i)]= feval(fun,tt,Globi,Outi,number,ax,varargin{:}); % run control protacol, varargin is inital state
            
            km1(:,1) = feval(fun,tt,Globi,Outi,number,ax,varargin{:});
            km1(:,2) = feval(fun,tt+num/2,Globi+num/2*km1(:,1),Outi,number,ax,varargin{:});
            km1(:,3) = feval(fun,tt+num/2,Globi+num/2*km1(:,2),Outi,number,ax,varargin{:});
            km1(:,4) = feval(fun,tt+num,Globi+num*km1(:,3),Outi,number,ax,varargin{:});

            Glob_last(:,number,ax) = Globi+(num/6)*( km1(:,1)+2* km1(:,2)+2* km1(:,3)+ km1(:,4));
        end
        

        %%%%%%%%%%%%%变队形判断%%%%%%%%%%%%%%%
%         disp( size(squeeze(Out(3:N2,number,1:3,i))) )
%         disp( size(squeeze(Out(2, number,1:3,i))) )
%         disp( size(Out(2, number,1:3,i)) )
         if mod(i,1)==0
            for j=1:size(bar,1)
                distance(j)=sqrt((Out(1,number,1,i)-bar(j,1))^2+(Out(1,number,2,i)-bar(j,2))^2);
                if (distance(j)<=1.95*bar(j,3) )
                    change_signal(number)=1;
                    break;
                else
                    change_signal(number)=0;
                end
            end
            if change_signal(number)== 1
                %zzy=squeeze(Out(3:N2,number,1:3,i))*[xydesire(number, 1), xydesire(number, 2), 0]';
                zzy=(  squeeze(Out(3:N2,number,1:3,i)) - squeeze(Out(1, number,1:3,i))'  )*[xydesire(number, 1), xydesire(number, 2), 0]';
                [~, L(:, number)]=sort(zzy);   %这里得到L，L是排列的索引号；
            end
         end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%搜索目标%%%%%%%%%%%%%%%
         if mod(i,10)==0
             for k=1:size(xdyd,1)
                 distance2=sqrt((Out(1,number,1,i)-xdyd(k,1))^2+(Out(1,number,2,i)-xdyd(k,2))^2);
                 if distance2 <= standard
                     des_flag(number ,k, i)= 1;
                     for j=1:N2-2   %构造重复的行；
                         xd_estimate(number ,k , j, i )=xdyd(k,1)+ rand(1,1) * (4 - (-4)) + (-4);    %在真实值的基础上，加入噪声；
                         yd_estimate(number ,k , j, i )=xdyd(k,2)+ rand(1,1) * (4 - (-4)) + (-4);
                     end
                     xdyd_mean = [mean(xd_estimate(number, k, : , i) ) , mean(yd_estimate(number, k, :, i ) )]; % target estimate 
                     set(handles.text_console,'String',['find target: [',num2str(xdyd_mean),']']);
%                      disp( squeeze(xd_estimate(number, k, : ,i) )  )
%                      disp( squeeze(yd_estimate(number, k, : ,i) )  )
%                     disp(xdyd_mean);
%                      disp('--------')
%                      plot(xdyd_mean(1),xdyd_mean(2),'*')
                 else
                     des_flag(number ,k, i)= 0;
                 end
             end
         end
         %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        hold on; 
        if ~mod(i,T_draw) %update pos handles for plot
            plot3(Out(1,number,1,i),Out(1,number,2,i),Out(1,number,3,i),'k.','MarkerSize',5);%plot leader trajectory
            hold on;
            if abs(Out(1,number,1,i))>scale||abs(Out(1,number,2,i))>scale
                scale=1.5*scale;
                axis([-scale,scale,-scale,scale]);%zoom figure scale
                grid on;
            end            
            set(handles.vr_leader(number),'xdata',Out(1,number,1,i),'ydata',Out(1,number,2,i),'zdata',Out(1,number,3,i));            
            set(handles.car(number),'xdata',Out(2,number,1,i),'ydata',Out(2,number,2,i),'zdata',Out(2,number,3,i));
            %set(handles.line(number),'X',[Out(2,number,1,i),Out(3,number,1,i)],'Y',[Out(2,number,2,i),Out(3,number,2,i)])
            for s=1:4 % update drone pos
                set(handles.drone(number,s),'xdata',Out(s+2,number,1,i),'ydata',Out(s+2,number,2,i),'zdata',Out(s+2,number,3,i));
                set(handles.dronelabell(number,s),'String',[]);%���ԭ��ǩ
                handles.dronelabell(number,s)=text(Out(s+2,number,1,i),Out(s+2,number,2,i),Out(s+2,number,3,i),[' ',num2str(s+2)]);
            end
        end
    end
    
    if ~mod(i,T_draw)
        drawnow limitrate; %draw plot in GUI
    end
    
    %%ROS communicate publish  
    if ROS_ON
        if ~mod(i,T_rospub)
            par_start =cputime;
            tmpStates= rosmessage('gazebo_msgs/ModelStates');
            for agent_num = 1:N*(N2-1)
                group_num = floor((agent_num-1)/(N2-1))+1; %group number
                s = mod(agent_num-1,N2-1)+2; % number in the group

    %             yaw=(group_num-1)/N*2*pi; % yaw angle
                yaw = atan2( xydesire(group_num, 2) , xydesire(group_num, 1) ); % yaw angle
                q = angle2quat(yaw,0, 0, 'ZYX');  
                    team_pose=rosmessage('geometry_msgs/Pose');      %λ�ó���xyz�����ø���Ԫ��
                    team_twist=rosmessage('geometry_msgs/Twist');    %�ٶ���Ĭ��0����Ҫ�ж���                
                    team_pose.Orientation.X=q(2);
                    team_pose.Orientation.Y=q(3);
                    team_pose.Orientation.Z=q(4);
                    team_pose.Orientation.W=q(1);
                    team_pose.Position.X=Out(s,group_num,1,i);
                    team_pose.Position.Y=Out(s,group_num,2,i);
                    team_pose.Position.Z=Out(s,group_num,3,i);

                    tmpPose(agent_num,1)=team_pose;%��ʱ����
                    tmpTwist(agent_num,1)= team_twist;                                  
            end  
            tmpStates.Name = tmp_msgs.Name;
            tmpStates.Pose = tmpPose;
            tmpStates.Twist = tmpTwist;
            Out_msgs(i-1,1)=tmpStates;      
            send(pub,Out_msgs(i-1));%publish ros message
            par_time = cputime-par_start; % Out_msgs һ�θ�ֵ��ʱ
            % pause(0.01); 
        end
        ite_time = cputime-t_start;   % iteration time 
    end
end

%rosshutdown;  %end ros
    


